{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Setup"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This notebook introduces the basic usage of the `RigidBodySim` package. Let's get started."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\u001b[1m\u001b[36mINFO: \u001b[39m\u001b[22m\u001b[36mRecompiling stale cache file /home/twan/code/RigidBodyDynamics/lib/v0.6/RigidBodySim.ji for module RigidBodySim.\n",
      "\u001b[39m"
     ]
    },
    {
     "data": {
      "text/html": [
       "<div id=\"interact-js-shim\">\n",
       "    <script charset=\"utf-8\">\n",
       "(function (IPython, $, _, MathJax) {\n",
       "    $.event.special.destroyed = {\n",
       "\tremove: function(o) {\n",
       "\t    if (o.handler) {\n",
       "\t\to.handler.apply(this, arguments)\n",
       "\t    }\n",
       "\t}\n",
       "    }\n",
       "\n",
       "    var OutputArea = IPython.version >= \"4.0.0\" ? require(\"notebook/js/outputarea\").OutputArea : IPython.OutputArea;\n",
       "\n",
       "    var redrawValue = function (container, type, val) {\n",
       "\tvar selector = $(\"<div/>\");\n",
       "\tvar oa = new OutputArea(_.extend(selector, {\n",
       "\t    selector: selector,\n",
       "\t    prompt_area: true,\n",
       "\t    events: IPython.events,\n",
       "\t    keyboard_manager: IPython.keyboard_manager\n",
       "\t})); // Hack to work with IPython 2.1.0\n",
       "\n",
       "\tswitch (type) {\n",
       "\tcase \"image/png\":\n",
       "            var _src = 'data:' + type + ';base64,' + val;\n",
       "\t    $(container).find(\"img\").attr('src', _src);\n",
       "\t    break;\n",
       "\tcase \"text/latex\":\n",
       "\t\tif (MathJax){\n",
       "\t\t\tvar math = MathJax.Hub.getAllJax(container)[0];\n",
       "\t\t\tMathJax.Hub.Queue([\"Text\", math, val.replace(/^\\${1,2}|\\${1,2}$/g, '')]);\n",
       "\t\t\tbreak;\n",
       "\t\t}\n",
       "\tdefault:\n",
       "\t    var toinsert = OutputArea.append_map[type].apply(\n",
       "\t\toa, [val, {}, selector]\n",
       "\t    );\n",
       "\t    $(container).empty().append(toinsert.contents());\n",
       "\t    selector.remove();\n",
       "\t}\n",
       "    }\n",
       "\n",
       "\n",
       "    $(document).ready(function() {\n",
       "\tfunction initComm(evt, data) {\n",
       "\t    var comm_manager = data.kernel.comm_manager;\n",
       "        //_.extend(comm_manager.targets, require(\"widgets/js/widget\"))\n",
       "\t    comm_manager.register_target(\"Signal\", function (comm) {\n",
       "            comm.on_msg(function (msg) {\n",
       "                var val = msg.content.data.value;\n",
       "                $(\".signal-\" + comm.comm_id).each(function() {\n",
       "                var type = $(this).data(\"type\");\n",
       "                if (typeof(val[type]) !== \"undefined\" && val[type] !== null) {\n",
       "                    redrawValue(this, type, val[type], type);\n",
       "                }\n",
       "                });\n",
       "                delete val;\n",
       "                delete msg.content.data.value;\n",
       "            });\n",
       "\t    });\n",
       "\n",
       "\t    // coordingate with Comm and redraw Signals\n",
       "\t    // XXX: Test using Reactive here to improve performance\n",
       "\t    $([IPython.events]).on(\n",
       "\t\t'output_appended.OutputArea', function (event, type, value, md, toinsert) {\n",
       "\t\t    if (md && md.reactive) {\n",
       "                // console.log(md.comm_id);\n",
       "                toinsert.addClass(\"signal-\" + md.comm_id);\n",
       "                toinsert.data(\"type\", type);\n",
       "                // Signal back indicating the mimetype required\n",
       "                var comm_manager = IPython.notebook.kernel.comm_manager;\n",
       "                var comm = comm_manager.comms[md.comm_id];\n",
       "                comm.then(function (c) {\n",
       "                    c.send({action: \"subscribe_mime\",\n",
       "                       mime: type});\n",
       "                    toinsert.bind(\"destroyed\", function() {\n",
       "                        c.send({action: \"unsubscribe_mime\",\n",
       "                               mime: type});\n",
       "                    });\n",
       "                })\n",
       "\t\t    }\n",
       "\t    });\n",
       "\t}\n",
       "\n",
       "\ttry {\n",
       "\t    // try to initialize right away. otherwise, wait on the status_started event.\n",
       "\t    initComm(undefined, IPython.notebook);\n",
       "\t} catch (e) {\n",
       "\t    $([IPython.events]).on('kernel_created.Kernel kernel_created.Session', initComm);\n",
       "\t}\n",
       "    });\n",
       "})(IPython, jQuery, _, MathJax);\n",
       "</script>\n",
       "    <script>\n",
       "        window.interactLoadedFlag = true\n",
       "       $(\"#interact-js-shim\").bind(\"destroyed\", function () {\n",
       "           if (window.interactLoadedFlag) {\n",
       "               console.warn(\"JavaScript required by Interact will be removed if you remove this cell or run using Interact more than once.\")\n",
       "           }\n",
       "       })\n",
       "       $([IPython.events]).on(\"kernel_starting.Kernel kernel_restarting.Kernel\", function () { window.interactLoadedFlag = false })\n",
       "   </script>\n",
       "</div>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "using RigidBodySim"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "`RigidBodySim` reexports all symbols from the following packages:\n",
    "* [`RigidBodyDynamics`](https://github.com/tkoolen/RigidBodyDynamics.jl)\n",
    "* [`RigidBodyTreeInspector`](https://github.com/rdeits/RigidBodyTreeInspector.jl)\n",
    "\n",
    "In addition, it reexports select symbols from packages in the [`DifferentialEquations`](https://github.com/JuliaDiffEq/DifferentialEquations.jl) ecosystem. To access additional `DifferentialEquations` features, simply add one or more of the following:\n",
    "```julia\n",
    "using DiffEqBase # basics\n",
    "using OrdinaryDiffEq # more ODE-related functionality\n",
    "using DiffEqCallbacks # callback library\n",
    "using DifferentialEquations # everything\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Model definition"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We first need to create a `RigidBodyDynamics.Mechanism` instance, representing the rigid body system we want to simulate. One way of creating a `Mechanism` is by parsing a [URDF](http://wiki.ros.org/urdf) file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "urdf = Pkg.dir(\"RigidBodySim\", \"test\", \"urdf\", \"Acrobot.urdf\")\n",
    "mechanism = parse_urdf(Float64, urdf)\n",
    "remove_fixed_tree_joints!(mechanism);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we'll create a `MechanismState` object, and set the initial joint positions and velocities:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "state = MechanismState(mechanism)\n",
    "shoulder, elbow = joints(mechanism)\n",
    "configuration(state, shoulder)[:] = 0.3\n",
    "configuration(state, elbow)[:] = 0.4\n",
    "velocity(state, shoulder)[:] = 1.\n",
    "velocity(state, elbow)[:] = 2.;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Creating an `ODEProblem`:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "`RigidBodySim` uses RigidBodyDynamics.jl to evaluate the equations of motion, and [OrdinaryDiffEq.jl](https://github.com/JuliaDiffEq/OrdinaryDiffEq.jl) for numerical integration (see [DifferentialEquations.jl](https://github.com/JuliaDiffEq/DifferentialEquations.jl) for documentation).\n",
    "\n",
    "`RigidBodySim` does not attempt to abstract away this dependence on the `DifferentialEquations` ecosystem, as doing so would necessarily expose only a subset of the functionality provided by `DifferentialEquations`, and require users familiar with the `DifferentialEquations` ecosystem to learn yet another API. Instead, `RigidBodySim` simply plugs into existing `DifferentialEquations` functionality, providing convenience methods and extensions.\n",
    "\n",
    "One example of such a convience method is the following `DiffEqBase.ODEProblem` constructor, which sets up (but doesn't solve) the initial value problem to be solved through numerical integration of the equations of motion:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "DiffEqBase.ODEProblem with uType Array{Float64,1} and tType Float64. In-place: true\n",
       "timespan: (0.0, 10000.0)\n",
       "u0: [0.3, 0.4, 1.0, 2.0]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "final_time = 10000.\n",
    "problem = ODEProblem(state, (0., final_time))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Visualization"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "`RigidBodySim` uses `RigidBodyTreeInspector` for visualization.\n",
    "\n",
    "We'll begin by starting a new `DrakeVisualizer` process (a separate visualizer window) if one isn't running already:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "any_open_visualizer_windows() || (new_visualizer_window(); sleep(1));"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For those familiar with `DrakeVisualizer`, note that `RigidBodySim.new_visualizer_window()` is different from `DrakeVisualizer.new_window()`: `RigidBodySim` provides a customized visualizer, with buttons that allow the user to interact with the simulation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we'll create a `DrakeVisualizer.Visualizer` for our mechanism, a handle which allows us to interact with the visualizer window. We'll then visualize the mechanism at the chosen initial state:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "vis = Visualizer(mechanism, parse_urdf(urdf, mechanism))\n",
    "settransform!(vis, state);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "# Simulating while visualizing"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We don't just want to visualize the initial state; we want to visualize the mechanism during the simulation as well. In addition, we want to be able to interact with the simulation process (for example, by terminating it using the 'stop' button in the visualizer).\n",
    "\n",
    "This functionality is implemented using a set of `DifferentialEquations` integrator callbacks (i.e., a `DiffEqBase.CallbackSet`). `RigidBodySim` provides a convenience `CallbackSet` constructor for these visualizer hooks:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "vis_callbacks = CallbackSet(vis, state);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can now start simulating by simply calling the `DiffEqBase.solve` function, making sure to pass in our visualizer callback set as the `callback` keyword argument.\n",
    "\n",
    "**Note**: just in time compilation may make it seem like nothing is happening for a few seconds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "sol = solve(problem, Tsit5(), abs_tol = 1e-10, dt = 0.05, callback = vis_callbacks);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Note**: to terminate the simulation early, press the 'stop' button in the visualizer. If you'd like to simulate indefinitely, until the stop button is pressed, simply set `final_time` above to `Inf`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Note**: a headless simulation can of course be performed by simply omitting the `callback` keyword argument. This may be somewhat faster than visualizing while simulating."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Visualization after the simulation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also play back the simulation results after the simulation is completed. RigidBodySim provides an overload for `RigidBodyTreeInspector.animate` that takes a `DiffEqBase.ODESolution`, returned by the `solve` function. Note that this `animate` function can be used even if the original simulation was performed headless (without visualization).\n",
    "\n",
    "Here's an example 5-second headless simulation, followed by playback at half speed:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "problem = ODEProblem(state, (0., 5.))\n",
    "sol = solve(problem, Vern7(), abs_tol = 1e-10, dt = 0.05);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "animate(vis, state, sol; realtime_rate = 0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "Like with simulation, the 'stop' button in the visualizer window can be used to terminate visualization early."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simulation with a controller in the loop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So far, we've only simulated a passive mechanism. We can also simulate with a controller in the loop."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Basics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The basic approach to controller-in-the-loop simulation is to pass in an a function of the form\n",
    "```julia\n",
    "function (τ::AbstractVector, t, state::MechanismState)\n",
    "    # set τ based on t and state...\n",
    "end\n",
    "```\n",
    "as an extra argument to the `ODEProblem` constructor. For example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "control! (generic function with 1 method)"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function control!(τ, t, state)\n",
    "    view(τ, velocity_range(state, shoulder))[:] = sin(t)\n",
    "    view(τ, velocity_range(state, elbow)) .= -configuration(state, shoulder)\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "controlproblem = ODEProblem(state, (0., 5.), control!)\n",
    "sol = solve(controlproblem, Vern7(), abs_tol = 1e-10, dt = 0.05)\n",
    "animate(vis, state, sol)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Simulating a digital controller"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the previous example, the function `control!` is called whenever the integrator queries the dynamics. There are cases where this is not desired. For example, a digital controller typically runs at some fixed frequency, independent of the integrator time step. For such cases, `RigidBodySim` provides the `PeriodicController` type:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "τ = similar(velocity(state))\n",
    "Δt = 1e-3 # time between calls to the `control!` function\n",
    "controller = PeriodicController(τ, Δt, control!);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which can be passed into the `ODEProblem` constructor in similar fashion:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "DiffEqBase.ODEProblem with uType Array{Float64,1} and tType Float64. In-place: true\n",
       "timespan: (0.0, 5.0)\n",
       "u0: [0.0121495, 0.471529, -1.43205, -0.50953]"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "periodic_control_problem = ODEProblem(state, (0., 5.), controller)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Under the hood, this creates an `ODEProblem` that contains a [`DiffEqCallbacks.PeriodicCallback`](http://docs.juliadiffeq.org/stable/features/callback_library.html#PeriodicCallback-1), which ensures that the `control!` function is called at the desired rate."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "sol = solve(periodic_control_problem, Tsit5(), abs_tol = 1e-10, dt = 0.05, callback = vis_callbacks);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and animate the results:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "animate(vis, state, sol)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.2",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
